#include "pid_controller.h"
#include <algorithm>
#include <iostream>
#include <opencv2/opencv.hpp>
#include <chrono>
#include <vector>
#include <thread>
#include <atomic>
#include <fstream>
#include <string>
#include "C_H.h"

// 转向环 PID 控制器的全局变量（PD 控制）
float KP = 10.0, KD = 20.0,GKD=0.0;
// float Curve_PID[3]={7.0,17,0},Loc_PID[3]={3,6},Locd_PID[3]={3,7},Ramp_PID[3]={3,7},Huan1_PID[3]={6.9,16,0},Huan2_PID[3]={6.9,16,0},Huan3_PID[3]={6.9,16,0};
float Curve_PID[3]={1.15,2.9,0},Loc_PID[3]={0.6,1.8},Locd_PID[3]={0.6,1.8},Ramp_PID[3]={0.6,1.8},Huan1_PID[3]={1.15,2.8,0},Huan2_PID[3]={1.15,2.8,0},Huan3_PID[3]={1.15,2.8,0};
int turn_error, turn_lastError, turn_output;
int duoji_p,duoji_d,duoji_kd;
int turn_setPoint = 0, turn_outputLimit = SERVO_PWM_MAX;
float derivative;
float v_dif;

float speed_l_prevError,speed_r_prevError;

float delta_output_l;
float delta_output_r;

//30 0.9 4000 760
// 右轮速度环 PID 控制器的全局变量（PI 控制）
//float speed_r[0]=30, speed_r[1]=0.9;
float speed_r[3]={30,0.9,0};
float speed_r_w[3]={30,0.9,0};
float speed_r_d[3]={0,0,0};
int speed_r_error, speed_r_lastError, speed_r_integral, speed_r_output,speed_r_derivative;
int speed_i_r_output;
int speed_p_r_output;
int speed_d_r_output;
float speed_p,speed_i,speed_d; 
int speed_r_setPoint, speed_r_integralLimit=4000, speed_r_outputLimit=8000;
int speed_r_integralLimit_zhi = 4000,speed_r_integralLimit_wan = 4000;
// 左轮速度环 PID 控制器的全局变量（PI 控制）
//float speed_l[0]=30, speed_l[1]=0.9, speed_l[2]=0;
float speed_l[3]={20,0.9,0};
float speed_l_w[3]={20,0.9,0};
float speed_l_d[3]={0,0,0};
int speed_l_error, speed_l_lastError, speed_l_integral, speed_l_output,speed_l_derivative;
int speed_i_l_output;
int speed_p_l_output;
int speed_d_l_output;
int speed_l_setPoint, speed_l_integralLimit=4000, speed_l_outputLimit=8000;
int speed_l_integralLimit_zhi = 3000,speed_l_integralLimit_wan = 3000;
int speed_setPoint=400;
int speed_l_integralLimit_pwm = 4500;
int speed_r_integralLimit_pwm = 4500;

float length = 0.199;   // 车长 19.90cm
float width = 0.076;    // 车宽一半 
float angle_p, R;       // 阿克曼转向角比例和转向半径
float ackerman_al = 28;//38; // 左转内轮最大角度27
float ackerman_bl = 22;//33; // 左转外轮最大角度21
float ackerman_ar = 28;//38; // 右转内轮最大角度29
float ackerman_br = 22;//33; // 右转外轮最大角度23

double ackerman_left=1;  // 左轮差速比例
double ackerman_right=1; // 右轮差速比例

float ackerman_k=1; // 可调半径比例

int sten_pwml_max=SERVO_PWM_MAX; // 左转舵机最大pw..m
int sten_pwmr_max=SERVO_PWM_MAX; // 右转舵机最大pwm

int wideth = 15;
int delt_v;


float clamp(float value, float min, float max) {
    return (value < min) ? min : (value > max) ? max : value;
}

void Ackerman_chasu(int duoji_pwm)
{
    float R1, R2, R;
    if (duoji_pwm > 0)
    {
        //printf("cahsu111");
        angle_p = fabsf(1.0 * duoji_pwm / sten_pwml_max);//0.5
        if(zhushi_flag)printf("angle_p:%lf\r\n", angle_p);
        R1 = (length / tanf(ackerman_al * angle_p * pii / 180)) + width;
        R2 = (length / tanf(ackerman_bl * angle_p * pii / 180)) - width;
        R = ((R1 + R2) / 2) * ackerman_k;
        if(zhushi_flag)
        {
            printf("R1:%lf\r\n", R1);
            printf("R2:%lf\r\n", R2);
            printf("R:%lf\r\n", R);
        }
        ackerman_left = (R - width) / R;
        ackerman_right = (R + width) / R;
    }
    else if (duoji_pwm < 0)
    {
        //printf("cahsu222");
        angle_p = fabsf(1.0 * duoji_pwm / sten_pwmr_max);
        if(zhushi_flag)printf("angle_p:%lf\r\n", angle_p);
        R1 = (length / tanf(ackerman_ar * angle_p * pii / 180)) + width;
        R2 = (length / tanf(ackerman_br * angle_p * pii / 180)) - width;
        R = ((R1 + R2) / 2) * ackerman_k;
        if(zhushi_flag)
        {
            printf("R1:%lf\r\n", R1);
            printf("R2:%lf\r\n", R2);
            printf("R:%lf\r\n", R);
        }

        ackerman_left = (R + width) / R;
        ackerman_right = (R - width) / R;
    }
    else
    {
        //printf("cahsu333");
        ackerman_left = 1;
        ackerman_right = 1;
    }
    // ackerman_left = 1;
    // ackerman_right = 1;
    if(zhushi_flag)
    {
        printf("ackerman_left:%lf\r\n", ackerman_left);
        printf("ackerman_right:%lf\r\n", ackerman_right);
    }

}


float TurnPID_Calculate(int currentValue) {
    // 计算当前偏差
    turn_error = turn_setPoint - currentValue;

    // 计算微分项
    derivative = turn_error - turn_lastError;

    KP=Curve_PID[0];
    KD=Curve_PID[1];
    GKD=Curve_PID[2];

    duoji_p = KP * turn_error;
    duoji_d = KD * derivative;
    duoji_kd = (-1)*GKD * imu_gyro_z/100;

    //turn_output = duoji_p+duoji_d+duoji_kd;
    turn_output = duoji_p+duoji_d;


    // 输出限幅
    if (turn_output > turn_outputLimit) turn_output = turn_outputLimit;
    if (turn_output < -turn_outputLimit) turn_output = -turn_outputLimit;

    // 更新上一次误差
    turn_lastError = turn_error;

    return turn_output;
}



float SpeedLPID_Calculate_wei(int speed_setPoint, int currentValue) {
    speed_l_error = speed_setPoint - currentValue;
    
    // 计算积分项
    speed_l_integral += speed_l_error;
    speed_l_derivative = speed_l_error - speed_l_lastError;

    if(Road_flag == 1)
    {
        speed_l_integralLimit = speed_l_integralLimit_zhi;
    }
    else
    {
        speed_l_integralLimit = speed_l_integralLimit_wan;
    }
    // 积分限幅
    if (speed_l_integral > speed_l_integralLimit) speed_l_integral = speed_l_integralLimit;
    if (speed_l_integral < -speed_l_integralLimit) speed_l_integral = -speed_l_integralLimit;

    if(break_flag == 0)
    {
        speed_p = speed_l[0];
        speed_i = speed_l[1];
        speed_d = speed_l[2];
    }
    else
    {
        speed_p = 3;
        speed_i = 0;
        speed_d = 0;
    }

    speed_p_l_output = speed_p * speed_l_error;
    speed_i_l_output = speed_i * speed_l_integral;
    speed_d_l_output = speed_d * speed_l_derivative;


    speed_l_output = speed_p_l_output + speed_i_l_output + speed_d_l_output;
    
    // 输出限幅
    if (speed_l_output > speed_l_outputLimit) speed_l_output = speed_l_outputLimit;
    if (speed_l_output < -speed_l_outputLimit) speed_l_output = -speed_l_outputLimit;


    // 更新上一次误差
    speed_l_lastError = speed_l_error;

    if(zhushi_flag) {
        printf("speed_l_output:%d\r\n", speed_l_output);
        printf("speed_l_integral:%d\r\n", speed_l_integral);
        printf("speed_l_error:%d\r\n", speed_l_error);
        printf("speed_p_l_output:%d\r\n", speed_p_l_output);
        printf("speed_i_l_output:%d\r\n", speed_i_l_output);
    }

    return speed_l_output;
}


float SpeedRPID_Calculate_wei(int speed_setPoint, int currentValue) {
    speed_r_error = speed_setPoint - currentValue;
    
    // 计算积分项
    speed_r_integral += speed_r_error;
    speed_r_derivative = speed_r_error - speed_r_lastError;


    if(Road_flag == 1)
    {
        speed_r_integralLimit = speed_r_integralLimit_zhi;
    }
    else
    {
        speed_r_integralLimit = speed_r_integralLimit_wan;
    }
    // 计算微分项
    // 积分限幅
    if (speed_r_integral > speed_r_integralLimit) speed_r_integral = speed_r_integralLimit;
    if (speed_r_integral < -speed_r_integralLimit) speed_r_integral = -speed_r_integralLimit;

    if(break_flag == 0)
    {
        speed_p = speed_r[0];
        speed_i = speed_r[1];
        speed_d = speed_r[2];
    }
    else
    {
        speed_p = 3;
        speed_i = 0;
        speed_d = 0;
    }

    speed_p_r_output = speed_p * speed_r_error;
    speed_i_r_output = speed_i * speed_r_integral;
    speed_d_r_output = speed_d * speed_r_derivative;


    speed_r_output = speed_p_r_output + speed_i_r_output + speed_d_r_output;

    // 输出限幅
    if (speed_r_output > speed_r_outputLimit) speed_r_output = speed_r_outputLimit;
    if (speed_r_output < -speed_r_outputLimit) speed_r_output = -speed_r_outputLimit;
 

    // 更新上一次误差
    speed_r_lastError = speed_r_error;

    if(zhushi_flag) {
        printf("speed_r_output:%d\r\n", (int)speed_r_output);
        printf("speed_r_integral:%d\r\n", (int)speed_r_integral);
        printf("speed_r_error:%d\r\n", (int)speed_r_error);
        printf("speed_p_r_output:%d\r\n", (int)speed_p_r_output);
        printf("speed_i_r_output:%d\r\n", (int)speed_i_r_output);
    }

    return speed_r_output;
}



void w_chasu(void)
{
    delt_v = ackerman_k * imu_gyro_z * wideth / 2; 
    if(delt_v > 2000) delt_v = 2000;
    else if(delt_v < -2000) delt_v = -2000;
    pid_output_l = pid_speed_output_l - delt_v;
    pid_output_r = pid_speed_output_r + delt_v;

}

int image_error_last = 0;
float Kp,Kd,output;

//舵机pid控制函数
float pid_servo_mohu(int image_error)
 {
    float E=0,EC=0;

    derivative = image_error - image_error_last;
 
    E  = image_error;
    EC = derivative;
 
    
 
    //kp_m为动态的Kp最大值，kd_m为动态的最大Kd值
    // kp_m = 11.8 ;
    // kd_m = 13.0  ;
 
    Kp = KP_Fuzzy(E,EC);
    Kd = 20;//Kd_Fuzzy(EC) ;
 
    output = -( Kp*E+Kd*(EC) );
    image_error_last = image_error;

    return output;

 }